#include "SoftwareSerial.h"
#include "core_debug.h"

#define OVERSAMPLE 3
#define HALFDUPLEX_SWITCH_DELAY 5

#if !defined(TIMER_SERIAL)
#define TIMER_SERIAL TIM4
#else
#error No suitable timer found for SoftwareSerial, define TIMER_SERIAL in variant.h
#endif

HardwareTimer SoftwareSerial::timer(TIMER_SERIAL);
SoftwareSerial *SoftwareSerial::active_listener = nullptr;
SoftwareSerial *volatile SoftwareSerial::active_out = nullptr;
SoftwareSerial *volatile SoftwareSerial::active_in = nullptr;
int32_t SoftwareSerial::tx_tick_cnt = 0;
volatile int32_t SoftwareSerial::rx_tick_cnt = 0;
uint32_t SoftwareSerial::tx_buffer = 0;
int32_t SoftwareSerial::tx_bit_cnt = 0;
uint32_t SoftwareSerial::rx_buffer = 0;
int32_t SoftwareSerial::rx_bit_cnt = -1;
uint32_t SoftwareSerial::cur_speed = 0;

inline void SoftwareSerial::handleInterrupt()
{
    if (active_in)
    {
        active_in->recv();
    }
    
    if (active_out)
    {
        active_out->send();
    }
  
}

SoftwareSerial::SoftwareSerial(uint16_t receivePin, uint16_t transmitPin, bool inverse_logic)
    : _receivePin(receivePin)
    , _transmitPin(transmitPin)
    , _receivePinPort(REF_GPIO_PORT(digitalPinToPinName(receivePin)))
    , _receivePinNumber(REF_GPIO_PIN(digitalPinToPinName(receivePin)))
    , _transmitPinPort(REF_GPIO_PORT(digitalPinToPinName(transmitPin)))
    , _transmitPinNumber(REF_GPIO_PIN(digitalPinToPinName(transmitPin)))
    , _speed(0)
    , _buffer_overflow(false)
    , _inverse_logic(inverse_logic)
    , _half_duplex(receivePin == transmitPin)
    , _output_pending(0)
    , _receive_buffer_tail(0)
    , _receive_buffer_head(0)
{
    if ((receivePin < NUM_DIGITAL_PINS) || (transmitPin < NUM_DIGITAL_PINS))
    {
        set_GPIO_Port_Clock(CH_PORT(digitalPinToPinName(transmitPin)));
        set_GPIO_Port_Clock(CH_PORT(digitalPinToPinName(receivePin)));
    }
    else
    {
        _Error_Handler("ERROR: invalid pin number\n", -1);
    }
}

SoftwareSerial::~SoftwareSerial()
{
    end();
}

void SoftwareSerial::begin(long speed)
{
#ifdef FORCE_BAUD_RATE
    speed = FORCE_BAUD_RATE;
#endif

    _speed = speed;
    if (!_half_duplex)
    {
        setTX();
        setRX();
        listen();
    }
    else
    {
        setTX();
    }
}

bool SoftwareSerial::listen()
{
    if (active_listener != this)
    {
        while (active_out)
            ;
        active_listener->stopListening();
        rx_tick_cnt = 1;
        rx_bit_cnt = -1;
        setSpeed(_speed);
        active_listener = this;
        if (!_half_duplex)
        {
            active_in = this;
        }
        return true;
    }
    return false;
}

void SoftwareSerial::end()
{
    stopListening();
}

bool SoftwareSerial::stopListening()
{
    if (active_listener == this)
    {
        while (active_out)
            ;

        if (_half_duplex)
        {
            setRXTX(false);
        }
        active_listener = nullptr;
        active_in = nullptr;
        // turn off ints
        setSpeed(0);
        return true;
    }
    return false;
}

int SoftwareSerial::peek()
{
    if (_receive_buffer_head == _receive_buffer_tail)
    {
        return -1;
    }
    return _receive_buffer[_receive_buffer_head];
}

size_t SoftwareSerial::write(uint8_t byte)
{
    _output_pending = 1;
    while (active_out)
        ;
    // add start and stop bits.
    tx_buffer = byte << 1 | 0x200;
    if (_inverse_logic)
    {
        tx_buffer = ~tx_buffer;
    }
    tx_bit_cnt = 0;
    tx_tick_cnt = OVERSAMPLE;

    setSpeed(_speed);
    if (_half_duplex)
    {
        setRXTX(false);
    }
    _output_pending = 0;
    // make us active
    active_out = this;
    return 1;
}

int SoftwareSerial::read()
{
    if (_receive_buffer_head == _receive_buffer_tail)
    {
        return -1;
    }

    // Read from "head"
    uint8_t d = _receive_buffer[_receive_buffer_head]; // grab next byte
    _receive_buffer_head = (_receive_buffer_head + 1) % _SS_MAX_RX_BUFF;
    return d;
}

int SoftwareSerial::available()
{
    return (_receive_buffer_tail + _SS_MAX_RX_BUFF - _receive_buffer_head) % _SS_MAX_RX_BUFF;
}

void SoftwareSerial::flush()
{
    noInterrupts();
    _receive_buffer_head = _receive_buffer_tail = 0;
    interrupts();
}

void SoftwareSerial::setInterruptPriority(uint32_t preemptPriority, uint32_t subPriority) 
{
    timer.setInterruptPriority(preemptPriority, subPriority);
}

void SoftwareSerial::send()
{

    if (--tx_tick_cnt <= 0)
    { // if tx_tick_cnt > 0 interrupt is discarded. Only when tx_tick_cnt reach 0 we set TX pin.
        if (tx_bit_cnt++ < 10)
        { // tx_bit_cnt < 10 transmission is not fiisehed (10 = 1 start +8 bits + 1 stop)
            // send data (including start and stop bits)
            if (tx_buffer & 1)
            {
                GPIO_SetBits(_transmitPinPort, _transmitPinNumber);
            }
            else
            {
                GPIO_ResetBits(_transmitPinPort, _transmitPinNumber);
            }
            tx_buffer >>= 1;
            tx_tick_cnt = OVERSAMPLE; // Wait OVERSAMPLE tick to send next bit
        }
        else
        { // Transmission finished
            tx_tick_cnt = 1;
            if (_output_pending)
            {
                active_out = nullptr;

                // When in half-duplex mode, wait for HALFDUPLEX_SWITCH_DELAY bit-periods after the byte has
                // been transmitted before allowing the switch to RX mode
            }
            else if (tx_bit_cnt > 10 + OVERSAMPLE * HALFDUPLEX_SWITCH_DELAY)
            {
                if (_half_duplex && active_listener == this)
                {
                    setRXTX(true);
                }
                active_out = nullptr;
            }
        }
    }
}

void SoftwareSerial::recv()
{
    if (--rx_tick_cnt <= 0)
    { // if rx_tick_cnt > 0 interrupt is discarded. Only when rx_tick_cnt reach 0 RX pin is considered
        bool inbit = GPIO_ReadInputDataBit(_receivePinPort, _receivePinNumber) ^ _inverse_logic;
        
        if (rx_bit_cnt == -1)
        { // rx_bit_cnt = -1 :  waiting for start bit
            if (!inbit)
            {
                // core_debug("got start bit [%X] [%02X] %d\n",  _receivePinPort , _receivePinNumber, inbit);
                // got start bit
                rx_bit_cnt = 0;               // rx_bit_cnt == 0 : start bit received
                rx_tick_cnt = OVERSAMPLE + 1; // Wait 1 bit (OVERSAMPLE ticks) + 1 tick in order to sample RX pin in the middle of the edge (and not too close to the edge)
                rx_buffer = 0; 
            }
            else
            {
                rx_tick_cnt = 1; // Waiting for start bit, but we don't get right level. Wait for next Interrupt to ckech RX pin level
            }
        }
        else if (rx_bit_cnt >= 8)
        { // rx_bit_cnt >= 8 : waiting for stop bit
            if (inbit)
            {
                // core_debug("stop bit read complete [%X] [%02X] %d\n",  _receivePinPort , _receivePinNumber, inbit);
                // stop bit read complete add to buffer
                uint8_t next = (_receive_buffer_tail + 1) % _SS_MAX_RX_BUFF;
                if (next != _receive_buffer_head)
                {
                    // save new data in buffer: tail points to where byte goes
                    _receive_buffer[_receive_buffer_tail] = rx_buffer; // save new byte
                    _receive_buffer_tail = next;
                }
                else
                { // rx_bit_cnt = x  with x = [0..7] correspond to new bit x received
                    _buffer_overflow = true;
                }
            }
            // Full trame received. Resart wainting for sart bit at next interrupt
            rx_tick_cnt = 1;
            rx_bit_cnt = -1;
        }
        else
        {
            // data bits
            rx_buffer >>= 1;
            if (inbit)
            {
                rx_buffer |= 0x80;
            }
            rx_bit_cnt++;             // Preprare for next bit
            rx_tick_cnt = OVERSAMPLE; // Wait OVERSAMPLE ticks before sampling next bit
        }
    }
}

void SoftwareSerial::setTX()
{
    if (_inverse_logic)
    {
        GPIO_ResetBits(_transmitPinPort, _transmitPinNumber);
    }
    else
    {
        GPIO_SetBits(_transmitPinPort, _transmitPinNumber);
    }
    pinMode(_transmitPin, OUTPUT);
}

void SoftwareSerial::setRX()
{
    pinMode(_receivePin, _inverse_logic ? INPUT_PULLDOWN : INPUT_PULLUP);
}

void SoftwareSerial::setRXTX(bool input)
{
    if (_half_duplex)
    {
        if (input)
        {
            if (active_in != this)
            {
                setRX();
                rx_bit_cnt = -1; // rx_bit_cnt = -1 :  waiting for start bit
                rx_tick_cnt = 2; // 2 : next interrupt will be discarded. 2 interrupts required to consider RX pin level
                active_in = this;
            }
        }
        else
        {
            if (active_in == this)
            {
                setTX();
                active_in = nullptr;
            }
        }
    }
}

void SoftwareSerial::setSpeed(uint32_t speed)
{
    if (speed != cur_speed)
    {
        timer.pause();

        if (speed != 0)
        {
            uint32_t clock_rate, cmp_value;
            clock_rate = timer.getTimerClkFreq();
            int pre = 1;

            do
            {
                cmp_value = clock_rate / (speed * OVERSAMPLE);
                if (cmp_value >= UINT16_MAX)
                {
                    clock_rate = clock_rate / 2;
                    pre *= 2;
                }
            } while (cmp_value >= UINT16_MAX);
            core_debug("Speed pre: %d  cmp: %d", pre, cmp_value);
            timer.setPrescaleFactor(pre);
            timer.setOverflow(cmp_value);
            timer.setCount(0);
            timer.attachInterrupt(&handleInterrupt);
            timer.resume();
        }
        else
        {
            timer.detachInterrupt();
        }
        cur_speed = speed;

    }
}
